"""
Demo der viser en roterende 3D kube ved brug af 3D rotationsmatrixer og Pygame
Tilføj evt. koordinatakser under "EGEN KODE".

Kameramodel kunne tilføjes for at håndtere perspektiv projektion (4x4 matrix)
Andre transformationer (skalering, translation) kunne også tilføjes
Se fx https://gamemath.com/book/matrixtransforms.html
"""

from gym_cas import Matrix, cos, sin
import pygame


# 3D rotationsmatrixer om de tre koordinatakser.
# Bemærk ligheden med 2D rotationsmatrixen
def rot_x(v: float) -> Matrix:
    """3x3 rotationsmatrix om x-aksen med vinkel v (radianer)"""
    return Matrix([[1, 0, 0], 
                   [0, cos(v), -sin(v)], 
                   [0, sin(v), cos(v)]])


def rot_y(v: float) -> Matrix:
    """3x3 rotationsmatrix om y-aksen med vinkel v (radianer)"""
    return Matrix([[cos(v), 0, sin(v)], 
                   [0, 1, 0], 
                   [-sin(v), 0, cos(v)]])


def rot_z(v: float) -> Matrix:
    """3x3 rotationsmatrix om z-aksen med vinkel v (radianer)"""
    return Matrix([[cos(v), -sin(v), 0], 
                   [sin(v), cos(v), 0], 
                   [0, 0, 1]])


# Kubens 8 hjørnepunkter (3x8 matrix, hver kolonne er et punkt)
cube_vertices = Matrix(
    [
        [-1, 1, 1, -1, -1, 1, 1, -1],  # x koordinater
        [-1, -1, 1, 1, -1, -1, 1, 1],  # y koordinater
        [-1, -1, -1, -1, 1, 1, 1, 1],  # z koordinater
    ]
)

# Kubens kanter (12 kanter der forbinder de 8 hjørner ved index)
cube_edges = [
    [0, 1],
    [1, 2],
    [2, 3],
    [3, 0],
    [4, 5],
    [5, 6],
    [6, 7],
    [7, 4],
    [0, 4],
    [1, 5],
    [2, 6],
    [3, 7],
]

# EGEN KODE: Lav punkter (søjler) til koordinatakser
axes_vertices = Matrix(
    [
        [0],
        [0],
        [0]
        
    ]
)

# EGEN KODE: Lav kanter til koordinatakser
axes_edges = [

]

# Visualisering med Pygame

pygame.init()
pixels_per_unit = 100
xm, ym = 3 * pixels_per_unit, 3 * pixels_per_unit
screen = pygame.display.set_mode((xm * 2, ym * 2))
clock = pygame.time.Clock()

running = True
while running:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

    screen.fill("black")

    # Opdater vinkler baseret på tid
    t = pygame.time.get_ticks() * 0.002
    angle_x, angle_y, angle_z = t * 0.7, t * 1.0, t * 0.3

    # Beregning af samlet rotationsmatrix ved hjælp af matrixmultiplikation ZYX (dermed roteres først om x, så y, så z)
    rotation = rot_z(angle_z) * rot_y(angle_y) * rot_x(angle_x)

    # Beregning af roterede kubes hjørner hvor hver søjle er et punkt (3x3 matrix * 3x8 matrix = 3x8 matrix)
    rotated_vertices = rotation * cube_vertices

    # Beregning af roterede koordinatakser
    rotated_axes = rotation * axes_vertices

    # Tegn kanter
    for start, end in cube_edges:
        start = (
            xm + int(rotated_vertices.col(start)[0] * pixels_per_unit),
            ym + int(rotated_vertices.col(start)[1] * pixels_per_unit),
        )
        end = (
            xm + int(rotated_vertices.col(end)[0] * pixels_per_unit),
            ym + int(rotated_vertices.col(end)[1] * pixels_per_unit),
        )
        pygame.draw.line(screen, "blue", start, end, 2)

    # Tegn hjørner
    for i in range(rotated_vertices.cols):
        point = (
            xm + int(rotated_vertices.col(i)[0] * pixels_per_unit),
            ym + int(rotated_vertices.col(i)[1] * pixels_per_unit),
        )
        pygame.draw.circle(screen, "red", point, 5)

    # Tegn koordinat-akser
    colors = ["red", "green", "white"]
    color = iter(colors)
    for start, end in axes_edges:
        start = (
            xm + int(rotated_axes.col(start)[0] * pixels_per_unit),
            ym + int(rotated_axes.col(start)[1] * pixels_per_unit),
        )
        end = (
            xm + int(rotated_axes.col(end)[0] * pixels_per_unit),
            ym + int(rotated_axes.col(end)[1] * pixels_per_unit),
        )
        pygame.draw.line(screen, next(color), start, end, 2)

    pygame.display.flip()
    clock.tick(60)

pygame.quit()